#!/usr/bin/env python3

import numpy as np
import tf.transformations as tf

# 定义一个4x4变换矩阵
transform_matrix = np.array([
 [-1.22832223e-01 , 6.96309033e-01 ,-7.07153431e-01 , 1.82219365e-02],
 [-9.84795862e-01 ,-1.73715602e-01 , 6.96075949e-06 ,-1.39373385e-01],
 [-1.22838737e-01 , 6.96402628e-01,  7.07060128e-01 , 4.50668639e-01],
 [ 0.00000000e+00 , 0.00000000e+00 , 0.00000000e+00 , 1.00000000e+00]]


)

# 提取变换矩阵的位置部分
extracted_position_x = transform_matrix[0, 3]
extracted_position_y = transform_matrix[1, 3]
extracted_position_z = transform_matrix[2, 3]

# 提取变换矩阵的旋转矩阵部分
extracted_rotation_matrix = transform_matrix[:3, :3]

# 将旋转矩阵转换为四元数
extracted_quaternion = tf.quaternion_from_matrix(transform_matrix)

print("Extracted Position:")
print(f"x: {extracted_position_x}, y: {extracted_position_y}, z: {extracted_position_z}")
print("Extracted Quaternion:")
print(f"x: {extracted_quaternion[0]}, y: {extracted_quaternion[1]}, z: {extracted_quaternion[2]}, w: {extracted_quaternion[3]}")
